#include "inc/core/mover1d.h"
#include "inc/core/lib.h"
#include "stdio.h"
#include "stdlib.h"

//works only with only one source (one for E and one for B)
void calc_external_fields(ucgrid1d* grid, mover1d* mover, time t)
{
    int i = 0;
    floatdig pos;
    for(i = 0; i < grid->num_of_cells; i++)
    {
        ucgrid1d_get_cell_center_pos(grid, i, &pos);
        mover->sources->esources[0](t, pos, &(mover->E_ext[i]));
        mover->sources->bsources[0](t, pos, &(mover->B_ext[i]));
    }
}

void move(mover1d* mover, particles_set* set, time delta)
{
    int i = 0;
    floatdig* z_pos;
    floatdig* z_speed;
    
    //#pragma omp parallel for
    for (i = 0; i < set->num_of_particles; i++)
    {
        z_pos = &(set->particles[i].pos.v[Z]);
        if (set->particles[i].active == PARTICLE_ACTIVE)
        {
            z_speed = &(set->particles[i].speed.v[VZ]);
            *z_pos += (*z_speed)*delta;
        }
    }
}

void accelerate(mover1d* mover, ucgrid1d* grid, particles_set* set, time delta)
{
    int i = 0;
    floatdig z_pos;
    floatdig* E = grid->E;
    floatdig B = 0;
    int rc = 0;
    int lc = 0;
    floatdig rc_pos = 0;
    floatdig lc_pos = 0;
    floatdig k = 0;
    floatdig b = 0;
    floatdig Ep = 0;
    floatdig El;
    floatdig Er;
    floatdig Exl;
    floatdig Exr;
    floatdig Ex;
    floatdig vx_m;
    floatdig vy_m;
    floatdig vx_p;
    floatdig vy_p;
    floatdig A;
    floatdig A2;
    floatdig D;
    floatdig C;
    floatdig Bz;
    floatdig Bzl;
    floatdig Bzr;
    floatdig delta1 = (mover->qm)*delta;
    floatdig current_time = mover->dt*mover->cycles;

    //#pragma omp parallel for
    for (i = 0; i < set->num_of_particles; i++)
    {
        z_pos = set->particles[i].pos.v[Z];
        if (set->particles[i].active == PARTICLE_ACTIVE)
        {
            //ucgrid1d_interpolate_fields_in_point(grid, *z_pos, &E, &B);
            
            ucgrid1d_search_neighbor_cells(grid, z_pos, &lc, &rc);
            ucgrid1d_get_cell_center_pos(grid, lc, &lc_pos);
            ucgrid1d_get_cell_center_pos(grid, rc, &rc_pos);
           
            if((lc <  0) || (rc > grid->num_of_cells - 1))
            {
                printf("ALARAM\n");
            }
            if (lc == rc)
            {
                Ep = grid->E[lc];
                Ex = mover->E_ext[lc];
                Bz = mover->B_ext[lc];
            }
            else
            {
                El = grid->E[lc];
                Er = grid->E[rc];
                Exl = mover->E_ext[lc];
                Exr = mover->E_ext[rc];
                Bzl = mover->B_ext[lc];
                Bzr = mover->B_ext[rc];

                Ep =  El*((rc_pos - z_pos)/grid->parameters.cell_size) +
                        Er*((z_pos - lc_pos)/grid->parameters.cell_size);
                Ex = Exl*((rc_pos - z_pos)/grid->parameters.cell_size) +
                        Exr*((z_pos - lc_pos)/grid->parameters.cell_size);
                Bz = Bzl*((rc_pos - z_pos)/grid->parameters.cell_size) +
                        Bzr*((z_pos - lc_pos)/grid->parameters.cell_size);
            }
            A = mover->qmc*Bz*delta/2;
            A2 = A*A;
            D = 1 - A2;
            C = 1 + A2;
            
            set->particles[i].speed.v[VZ] +=  Ep * delta1;
            
            vx_m = set->particles[i].speed.v[VX] + Ex*delta1/2;
            vy_m = set->particles[i].speed.v[VY];
            
            vy_p = (vy_m*D - 2*A*vx_m)/C;
            vx_p = (vx_m*D + vy_m*2*A)/C;
            
            set->particles[i].speed.v[VX] = vx_p + Ex*delta1/2;
            set->particles[i].speed.v[VY] = vy_p;
        }
    }
}

/*API*/
int mover1d_setup_mover(mover1d_parameters* parameters, mover1d* mover)
{
    int num_of_particles = parameters->set->num_of_particles;
    floatdig grid_speed = 0;
	#ifdef VERBOSE
    printf("init mover\n");
    #endif
    if ((parameters == NULL)|| (mover == NULL))
    {
        return EXIT_FAILURE;
    }
    mover->cycles = 0;
    mover->dt = parameters->dt;
    mover->dx = parameters->grid->grid_cell_size;
    mover->qm = parameters->set->charge / parameters->set->mass;
    mover->qmc = mover->qm / LIGHT_SPEED;
    grid_speed = mover->dx/mover->dt;
    mover->sources = parameters->sources;
    mover->B_ext = (floatdig*)malloc((sizeof(*mover->B_ext))*parameters->grid->num_of_cells);
    mover->E_ext = (floatdig*)malloc((sizeof(*mover->E_ext))*parameters->grid->num_of_cells);
    #ifdef VERBOSE
    printf("normalize particles' speeds and coords using grid parameters\n");
    #endif
/*
    for (i = 0; i < num_of_particles; i++)
    {
        lib_divide_vector( &(parameters->set->particles[i].pos),
                          mover->dx,
                          &(parameters->set->particles[i].pos));
        lib_divide_vector( &(parameters->set->particles[i].speed),
                          mover->dx,
                          &(parameters->set->particles[i].speed));

        
    }
*/
    return EXIT_SUCCESS;
}

void mover1d_clear_mover(mover1d* mover)
{
    mover->qm = 0;
    mover->qmc = 0;
    free(mover->B_ext);
    free(mover->E_ext);
}

void mover1d_single_iteration(mover1d* mover, ucgrid1d* grid, particles_set* set)
{
    //evaluate electrostatic field from initial charge distrubution
#ifdef VERBOSE
    printf("Single iteration over particles has been started\n");
    printf("Evaluating electrostaic field from charge distribution\n");
    printf("Cycle number %i\n", mover->cycles);
#endif
    ucgrid1d_calc_charge_density_from_particles_distr(set, grid);
    ucgrid1d_calc_phi_from_charge_density(grid);
    ucgrid1d_calc_static_fields(grid);

    //evaluate external fields
#ifdef VERBOSE
    printf("Evaluating external fields\n");
#endif
    calc_external_fields(grid, mover, mover->cycles*mover->dt);

    if (mover->cycles == 0)
    {
#ifdef VERBOSE
        printf("Performing backtracing of particles\n");
#endif
        accelerate(mover, grid, set, -0.5*mover->dt);
    }

#ifdef VERBOSE
    printf("Accelerate and move particles...\n");
#endif
    accelerate(mover, grid, set, mover->dt);
    move(mover, set, mover->dt);
    mover->cycles++;
}


#ifdef DEBUG
    void mover1d_make_snapshot_of_ext_E(ucgrid1d* grid, mover1d* mover, char* filename)
    {
        FILE* to;
        int i = 0;
        to = fopen(filename, "w");
        for (i = 0; i < grid->num_of_cells; i++)
        {
            fprintf(to, "%d\t%e\n", i, mover->E_ext[i]);
        }
        fclose(to);
    }
#endif